﻿#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <random>
#include <vector>

#include <xy/alg/interpolator.h>
#include <xy/alg/projection.h>
#include <xy/utils/scope_profiler.h>

using namespace xy;
using namespace xy::alg;
using namespace xy::geom;
using namespace xy::math;

class TestProjection : public ::testing::Test
{
  public:
    // 初始化函数，其中初始化要测试的数据
    void SetUp() override
    {
        generator = std::mt19937(std::random_device{}());            // 使用 time(nullptr) 作为随机数生成器的种子
        uniform = std::uniform_real_distribution<double>(0.0, 10.0); // 在 [0,1] 区间内的均匀分布

        // 初始化曲线
        {
            int N = 100;
            std::vector<vec3> points;
            for (int i = 0; i < N; i++)
                points.push_back(vec3{uniform(generator), uniform(generator), uniform(generator)});

            interpolator_cubic_bspline<3> interpolator;
            curve = interpolator.interpolate(points, interpolator.Natural, interpolator.Natural, vec3{}, vec3{});
        }

        // 初始化曲面
        {
            auto f = [](double u, double v) -> vec3 { return vec3{u, v, sin(u + v) * cos(u - v)}; };
            // 20 x 30 个采样点
            int N = 10;
            int M = 15;
            std::vector<std::vector<vec3>> points(N, std::vector<vec3>(M));
            for (int i = 0; i < N; i++)
                for (int j = 0; j < M; j++)
                    points[i][j] = f(10.0 / (N - 1) * i, 10.0 / (M - 1) * j);
            surface = interpolator_cubic_bspline<3>::interpolate(points);
        }
    }

    void TearDown() override
    {
        // ScopeProfilerLog;
    }

    std::shared_ptr<bspline_curve<3>> curve;
    std::shared_ptr<bspline_surface<3>> surface;

    std::mt19937 generator;
    std::uniform_real_distribution<double> uniform;
};

// 测试点到曲线的投影函数
TEST_F(TestProjection, Point2Curve)
{
    int M = 100;
    int valid = 0;

    {
        ScopeProfilerNamed(Near);

        for (int i = 0; i < M; i++)
        {
            vec3 P{uniform(generator), uniform(generator), uniform(generator)};
            double t = 0.5;

            // 测试最近投影（不稳定，需要接近最近点）
            t = projection_point<3>::nearest(curve, P, t, numeric::distance);
            auto [Q, V1] = curve->d1(t);
            auto cosine = dot(V1.normalize(), (P - Q).normalize());
            auto distance = (P - Q).modulus();

            if (std::abs(cosine) < numeric::distance)
                valid++;
            // EXPECT_LE(cosine, numeric::distance);
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.90);

    valid = 0;

    {
        ScopeProfilerNamed(Ortho1);

        for (int i = 0; i < M; i++)
        {
            vec3 P{uniform(generator), uniform(generator), uniform(generator)};
            double t = 0.5;

            // 测试 1 阶正交投影（不稳定，需要接近最近点）
            t = projection_point<3>::orthogonal1(curve, P, t, numeric::distance);
            auto [Q, V1] = curve->d1(t);
            auto cosine = dot(V1.normalize(), (P - Q).normalize());
            auto distance = (P - Q).modulus();

            if (std::abs(cosine) < numeric::distance)
                valid++;
            // std::cout << distance << " " << cosine << std::endl;
            // EXPECT_LE(cosine, numeric::distance);
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.90);

    valid = 0;

    {
        ScopeProfilerNamed(Ortho2);

        for (int i = 0; i < M; i++)
        {
            vec3 P{uniform(generator), uniform(generator), uniform(generator)};
            double t = 0.5;

            // 测试 2 阶正交投影（较稳定）
            t = projection_point<3>::orthogonal2(curve, P, t, numeric::distance);
            auto [Q, V1] = curve->d1(t);
            auto cosine = dot(V1.normalize(), (P - Q).normalize());
            auto distance = (P - Q).modulus();

            if (std::abs(cosine) < numeric::distance)
                valid++;
            // std::cout << distance << " " << cosine << std::endl;
            // EXPECT_LE(cosine, numeric::distance);
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
}

// 测试点到曲线的平行投影函数
TEST_F(TestProjection, Point2Curve_parallel)
{
    ScopeProfilerNamed(Parallel);

    int M = 100;
    int valid = 0;

    auto [first, last] = curve->bound();
    std::uniform_real_distribution<double> tar(first, last);
    for (int i = 0; i < M; i++)
    {
        vec3 dir{uniform(generator), uniform(generator), uniform(generator)};
        double u = tar(generator);
        vec3 P = curve->d0(u) - dir;

        double t = 0.5;
        t = projection_point<3>::direction(curve, P, dir, t, 1e-2);
        auto Q = curve->d0(t);
        auto cosine = cross(dir.normalize(), (P - Q).normalize()).modulus();

        // std::cout << cosine << " " << sin(degrees_to_radians(0.05)) << std::endl;
        if (std::abs(cosine) < 1e-2)
            valid++;
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
}

// 测试点到曲面的投影函数
TEST_F(TestProjection, Point2Surface)
{
    auto [U1, U2] = surface->bound(0);
    auto [V1, V2] = surface->bound(1);

    int M = 100;
    int valid = 0;

    {
        ScopeProfilerNamed(Near);

        for (int i = 0; i < M; i++)
        {
            vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
            double u = 0.5, v = 0.5;

            auto uv = projection_point<3>::nearest(surface, P, u, v, numeric::distance);
            auto [Q, DU, DV] = surface->d1(uv);
            auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

            if (std::abs(angle) < numeric::distance)
                valid++;
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    {
        ScopeProfilerNamed(Parallel);
        vec3 O = surface->d0({0.25, 0.75});
        for (int i = 0; i < M; i++)
        {
            vec3 dir{uniform(generator), uniform(generator), uniform(generator)};
            auto P = O - dir;
            double u = 0.5, v = 0.5;

            auto uv = projection_point<3>::direction(surface, P, dir, u, v, numeric::distance);
            auto Q = surface->d0(uv);
            auto angle = cross(dir.normalize(), (P - Q).normalize()).modulus();

            if (std::abs(angle) < numeric::distance)
                valid++;
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    {
        ScopeProfilerNamed(Ortho1);
        for (int i = 0; i < M; i++)
        {
            vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
            double u = 0.5, v = 0.5;

            auto uv = projection_point<3>::orthogonal1(surface, P, u, v, numeric::distance);
            auto [Q, DU, DV] = surface->d1(uv);
            auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

            if (std::abs(angle) < numeric::distance)
                valid++;
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    {
        ScopeProfilerNamed(Ortho2);
        for (int i = 0; i < M; i++)
        {
            vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
            double u = 0.5, v = 0.5;

            auto uv = projection_point<3>::orthogonal2(surface, P, u, v, numeric::distance);
            auto [Q, DU, DV] = surface->d1(uv);
            auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

            if (std::abs(angle) < numeric::distance)
                valid++;
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    {
        ScopeProfilerNamed(Torus);

        // {1.38115 0.277605 0.66348 }
        // vec3 P{ 1.210816402930, 9.741359425732 ,1.293815732959 };
        // double u = 0.5, v = 0.5;
        // auto uv = torus<3>(surface, P, u, v, numeric::distance);
        // auto [Q, DU, DV] = surface->d1(uv.x(), uv.y());
        // auto angle = ((DU % DV).normalize() % (P - Q).normalize()).modulus();

        for (int i = 0; i < M; i++)
        {
            vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
            double u = 0.5, v = 0.5;

            auto uv = projection_point<3>::torus(surface, P, u, v, numeric::distance);
            auto [Q, DU, DV] = surface->d1(uv);
            auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

            // std::cout << i << " " << angle << std::endl;
            if (std::abs(angle) < numeric::distance)
                valid++;
        }
    }

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;
}

// // 测试曲线到曲面的投影函数
// TEST_F(TestProjection, Curve2Surface)
// {
//     double U1, U2, V1, V2;
//     surface->Bounds(U1, U2, V1, V2);

//     int valid = 0;
//     int T = 100;

//     TIMER_START(March)

//     std::vector<BPnt2> para;
//     ProjMat::March(curve, surface, 0, 1, T, xy_ABS_ACC, para);
//     double k = 1.0 / T;

//     TIMER_END(March)

//     for (int i = 0; i <= T; i++)
//     {
//         double t = k * i;
//         auto P = curve->d0(t);

//         vec3 Q;
//         BVec DU, DV;
//         surface->D1(para[i][0], para[i][1], Q, DU, DV);
//         auto N = (DU % DV).Normalized();

//         double angle = (N % (P - Q)).Modulus();
//         auto distance = P.Distance(Q);

//         auto u = para[i][0];
//         auto v = para[i][1];
//         // if (distance < xy_ABS_ACC || fabs(angle) < xy_ABS_ACC || (fabs(u - U1) < xy_ABS_ACC * 2) ||
//         //     (fabs(u - U2) < xy_ABS_ACC * 2) || (fabs(v - V1) < xy_ABS_ACC * 2) || (fabs(v - V2) < xy_ABS_ACC *
//         //     2)) valid++;
//         if (distance < xy_ABS_ACC || fabs(angle) < xy_ABS_ACC)
//             valid++;
//     }

//     dbg(1.0 * valid / T);
//     EXPECT_TRUE(1.0 * valid / T > 0.5);
//     valid = 0;
//     para.clear();

//     TIMER_START(ODE)

//     ProjMat::ODE(curve, surface, 0, 1, T, xy_ABS_ACC, para);

//     TIMER_END(ODE)

//     for (int i = 0; i <= T; i++)
//     {
//         double t = k * i;
//         auto P = curve->d0(t);

//         vec3 Q;
//         BVec DU, DV;
//         surface->D1(para[i][0], para[i][1], Q, DU, DV);
//         auto N = (DU % DV).Normalized();

//         double angle = (N % (P - Q)).Modulus();
//         auto distance = P.Distance(Q);

//         auto u = para[i][0];
//         auto v = para[i][1];
//         // if (distance < xy_ABS_ACC || fabs(angle) < xy_ABS_ACC || (fabs(u - U1) < xy_ABS_ACC * 2) ||
//         //     (fabs(u - U2) < xy_ABS_ACC * 2) || (fabs(v - V1) < xy_ABS_ACC * 2) || (fabs(v - V2) < xy_ABS_ACC *
//         //     2)) valid++;
//         if (distance < xy_ABS_ACC || fabs(angle) < xy_ABS_ACC)
//             valid++;
//     }

//     dbg(1.0 * valid / T);
//     EXPECT_TRUE(1.0 * valid / T > 0.7);
//     valid = 0;
// }